// 放在你的 .h 或 .cpp 最顶部，所有系统头文件之前
#ifndef _POSIX_C_SOURCE
#define _POSIX_C_SOURCE 200809L
#endif
#ifndef _GNU_SOURCE
#define _GNU_SOURCE
#endif


#pragma once
// ===================== tsync::LogicalClock with POSIX shm publisher =====================
// 依赖：Linux/Unix (shm_open/mmap)，C++17

#include <atomic>
#include <cstdint>
#include <chrono>
#include <algorithm>
#include <vector>
#include <utility>
#include <limits>
#include <cmath>
#include <string>
#include <cstdio>   // for perror


// --- POSIX shm ---
#include <sys/mman.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>

#include "time_source.h"   // 需要提供 TscSource::()

namespace tsync {

// =====================================================
// 共享锚点映射 (seqlock)：R_anchor / L_anchor / s_ns_per_ns
// =====================================================
namespace ipc {

struct alignas(64) ShmLcMap {
    std::atomic<uint64_t> seq;   // 偶数=稳定，奇数=写入中
    uint64_t R_anchor_ns;        // 原始单调时钟锚点（ns）
    int64_t  L_anchor_ns;        // 逻辑时钟锚点（ns）
    double   s_ns_per_ns;        // 当前斜率（ns/ns）
    uint32_t version;            // 结构版本
    uint32_t _pad{0};
};

class ShmLcPublisher {
public:
    explicit ShmLcPublisher(const std::string& name = "/lc_map_v1")
    : name_(name) { open_or_create(true); }

    ~ShmLcPublisher() { close_unmap(); }

    inline void publish(uint64_t R_anchor_ns,
                        int64_t  L_anchor_ns,
                        double   s_ns_per_ns,
                        uint32_t version = 1) noexcept {
        if (!ptr_) return;
        // seqlock：开始写（-> odd）
        ptr_->seq.fetch_add(1, std::memory_order_acq_rel);
        ptr_->R_anchor_ns = R_anchor_ns;
        ptr_->L_anchor_ns = L_anchor_ns;
        ptr_->s_ns_per_ns = s_ns_per_ns;
        ptr_->version     = version;
        // seqlock：结束写（-> even）
        ptr_->seq.fetch_add(1, std::memory_order_acq_rel);
    }

private:
    std::string name_;
    int fd_{-1};
    ShmLcMap* ptr_{nullptr};
    static constexpr size_t kSize = sizeof(ShmLcMap);

    void open_or_create(bool create) {
        int flags = O_RDWR; if (create) flags |= O_CREAT;
        fd_ = shm_open(name_.c_str(), flags, 0660);
        if (fd_ < 0) { perror("shm_open"); return; }
        if (create) {
            if (ftruncate(fd_, kSize) != 0) { perror("ftruncate"); }
        }
        void* p = mmap(nullptr, kSize, PROT_READ|PROT_WRITE, MAP_SHARED, fd_, 0);
        if (p == MAP_FAILED) { perror("mmap"); close(fd_); fd_ = -1; return; }
        ptr_ = reinterpret_cast<ShmLcMap*>(p);

        // 幂等初始化
        if (ptr_->seq.load(std::memory_order_relaxed) == 0) {
            ptr_->R_anchor_ns = 0;
            ptr_->L_anchor_ns = 0;
            ptr_->s_ns_per_ns = 1.0;
            ptr_->version     = 1;
        }
    }

    void close_unmap() {
        if (ptr_) { munmap((void*)ptr_, kSize); ptr_ = nullptr; }
        if (fd_ >= 0) { close(fd_); fd_ = -1; }
    }
};

} // namespace ipc

// ---- GCC6 兼容：缺失 std::clamp 时的简易替代 ----
namespace detail_compat {
template<typename T>
inline T clamp_compat(T v, T lo, T hi) {
    return v < lo ? lo : (v > hi ? hi : v);
}
}

// 逻辑时钟：支持相位偏移 + 追赶斜率（ns/ns），带 LSE(一次拟合)窗口
class LogicalClock {
public:
    // 构造：init_offset_ns 为初始相位偏移；enable_zero_base=true 表示启用“启动归零”
    inline explicit LogicalClock(int64_t init_offset_ns = 0, bool enable_zero_base = true)
        : offset_ns_(init_offset_ns),
          zero_base_(enable_zero_base) ,
          shm_pub_("/lc_map_v1")               // 共享段名称可改
    {
        const int64_t now_abs_ns = (int64_t)TscSource::now_ns();
        t0_ns_ = zero_base_ ? now_abs_ns : 0;

        anchor_tsc_ns_   = now_abs_ns;
        anchor_logic_ns_ = raw_monotonic_ns() + offset_ns_.load(std::memory_order_relaxed);

        // 追赶斜率初始锚点
        slope_anchor_raw_ns_.store(raw_monotonic_ns(), std::memory_order_relaxed);

        // 初始化 LSE 窗
        init_lsq_window(32);

        // 构造完成：发布一次锚点
        publish_anchor_();
    }

    // 读取“归零后”的本地单调时间（ns）
    inline int64_t raw_monotonic_ns() const noexcept {
        int64_t abs_tsc_ns = (int64_t)TscSource::now_ns();
        return zero_base_ ? (abs_tsc_ns - t0_ns_) : abs_tsc_ns;
    }

    // 读取逻辑时间（ns）
    inline int64_t now_ns() const noexcept {
        const int64_t raw = raw_monotonic_ns();
        return logical_at_raw_(raw);
    }

    // 逻辑相位（对 tau 取模）
    inline uint32_t phase_mod_tau(uint32_t tau_ns) noexcept {
        int64_t t = now_ns();
        int64_t r = (int64_t)(t % (int64_t)tau_ns);
        if (r < 0) r += tau_ns;
        return (uint32_t)r;
    }

    // === 偏移（相位） ===
    inline int64_t offset_ns() const noexcept {
        return offset_ns_.load(std::memory_order_relaxed);
    }
    inline void set_offset_ns(int64_t new_off_ns) noexcept {
        offset_ns_.store(new_off_ns, std::memory_order_relaxed);
        publish_anchor_();
    }
    inline void add_offset_step(int64_t delta_ns) noexcept {
        offset_ns_.fetch_add(delta_ns, std::memory_order_relaxed);
        publish_anchor_();
    }

    // ====== 重新归零 / 切换开关 ======
    inline void rebase_to_now() noexcept {
        const int64_t old_zero = t0_ns_;
               int64_t new_zero = (int64_t)TscSource::now_ns();
        const int64_t delta = new_zero - old_zero;
        offset_ns_.fetch_add(delta, std::memory_order_relaxed);
        t0_ns_ = new_zero;
        publish_anchor_();
    }
    inline void set_zero_base(bool enable) noexcept {
        if (enable == zero_base_) return;
        if (enable) { rebase_to_now(); zero_base_ = true; }
        else {
            const int64_t z = t0_ns_;
            offset_ns_.fetch_sub(z, std::memory_order_relaxed);
            t0_ns_ = 0; zero_base_ = false;
            publish_anchor_();
        }
    }

    // ====== 追赶斜率：成员 + 接口（用于“好轮/坏轮”的 s 设置） ======
    inline double slope_ns_per_ns() const noexcept {
        return slope_ns_per_ns_.load(std::memory_order_relaxed);
    }
    inline double slope_ppm() const noexcept {
        return slope_ns_per_ns_.load(std::memory_order_relaxed) * 1e6;
    }

    // —— 记录映射得到的 s 与 θ（t1_mid）——
    inline double  mapped_s()    const noexcept { return mapped_s_ns_per_ns_.load(std::memory_order_relaxed); }
    inline int64_t mapped_theta() const noexcept { return mapped_theta_ns_.load(std::memory_order_relaxed); }

    // 折叠旧斜率累计量 → 设新斜率，并把锚点移动到“现在”（保证 L(t) 连续）
    inline void set_catch_slope_ns_per_ns(double new_s) noexcept {
        const int64_t raw = raw_monotonic_ns();
        const double  old = slope_ns_per_ns_.load(std::memory_order_relaxed);
        const int64_t a   = slope_anchor_raw_ns_.load(std::memory_order_relaxed);
        if (a != 0 && old != 0.0) {
            const int64_t dt = raw - a;
            const int64_t fold = (int64_t)std::llround((double)dt * old);
            if (fold) offset_ns_.fetch_add(fold, std::memory_order_relaxed);
        }
        slope_ns_per_ns_.store(new_s, std::memory_order_relaxed);
        slope_anchor_raw_ns_.store(raw, std::memory_order_relaxed);

        // 斜率“提交点”：发布新的锚点（确保事务一致）
        publish_anchor_at_raw_(raw);
    }
    inline void set_catch_slope_ppm(double ppm) noexcept {
        set_catch_slope_ns_per_ns(ppm * 1e-6);
    }

    // ====== 好/坏轮：映射到 s_map = μ / (τ - t1_mid)；坏轮：过 A(R_A,L_A) 的拟合 ======

    // === 好轮：直接映射 s = μ / τ（不使用 t1_mid）===
    inline bool apply_mu_good_map(int64_t mu_ns,
                                  int64_t tau_ns,
                                  double  s_abs_max_ppm = 0.0,
                                  int     k_capacity     = 32) noexcept
    {
        if (lsq_cap_ != k_capacity) init_lsq_window(std::max(3, k_capacity));
        if (tau_ns <= 0) {
            // τ 非法，退回到坏轮拟合以避免除零/爆表
            return apply_mu_bad_fit(/*s_abs_max_ppm=*/s_abs_max_ppm, /*k_capacity=*/k_capacity);
        }

        // 1) 直接映射 s_map = μ / τ
        double s_map = (double)mu_ns / (double)tau_ns; // ns/ns

        // 可选：ppm 限幅
        if (s_abs_max_ppm > 0.0) {
            const double smax = s_abs_max_ppm * 1e-6;
            if (s_map >  smax) s_map =  smax;
            if (s_map < -smax) s_map = -smax;
        }

        // 业务规则：不允许负追赶（与原逻辑保持一致）
        if (s_map < 0) s_map = 0;

        // 2) 设置追赶斜率（保证 L 连续）
        set_catch_slope_ns_per_ns(s_map);

        // 3) 记录映射参数（θ不再有意义，保留为上次或置 0）
        mapped_s_ns_per_ns_.store(s_map, std::memory_order_relaxed);
        mapped_theta_ns_.store(0, std::memory_order_relaxed);

        // 4) 入样本（好轮）
        const int64_t R_now = raw_monotonic_ns();
        const int64_t L_now = now_ns();
        lsq_add_sample_(R_now, L_now, k_capacity);
        return true; // 标记为好轮
    }

    // === 坏轮：过 A(RA,LA) 的锚点拟合（与原“分母<=0”路径一致）===
    inline bool apply_mu_bad_fit(double s_abs_max_ppm = 0.0,
                                 int    k_capacity     = 32) noexcept
    {
        if (lsq_cap_ != k_capacity) init_lsq_window(std::max(3, k_capacity));

        const int64_t R_A = raw_monotonic_ns();
        const int64_t L_A = now_ns();

        long double a_fit;
        if (lsq_fit_slope_through_anchor_(R_A, L_A, a_fit)) {
            double s_fit = (double)(a_fit - 1.0L); // ns/ns

            // 可选：ppm 限幅
            if (s_abs_max_ppm > 0.0) {
                const double smax = s_abs_max_ppm * 1e-6;
                if (s_fit >  smax) s_fit =  smax;
                if (s_fit < -smax) s_fit = -smax;
            }

            set_catch_slope_ns_per_ns(s_fit);

            // 记录映射 s；θ保持上次值（如需显式标坏轮，可置 -1）
            mapped_s_ns_per_ns_.store(s_fit, std::memory_order_relaxed);
            // mapped_theta_ns_.store(-1, std::memory_order_relaxed);

            // 入样本（坏轮）
            lsq_add_sample_(R_A, L_A, k_capacity);
            return false; // 标记为坏轮
        }

        // 连拟合也失败：不改 s，但推进窗口
        lsq_add_sample_(R_A, L_A, k_capacity);
        return false;
    }

    // === 兼容包装（可保留，内部改为调用新实现）===
    inline bool apply_mu_from_t1mid_or_fit(int64_t mu_ns,
                                           int64_t /*t1_mid_ns*/,
                                           int64_t tau_ns,
                                           double  s_abs_max_ppm = 0.0,
                                           int     k_capacity     = 32) noexcept
    {
        return apply_mu_good_map(mu_ns, tau_ns, s_abs_max_ppm, k_capacity);
    }

    // 兼容接口：传入的是 t1_logic_ns（直接用逻辑时刻），内部转成 t1_mid = t1_logic_ns % τ
    inline bool apply_mu_from_t1_or_fit(int64_t mu_ns,
                                        int64_t t1_logic_ns,
                                        int64_t tau_ns,
                                        double  s_abs_max_ppm = 0.0,
                                        int     k_capacity     = 32) noexcept
    {
        int64_t t1_mid_ns = t1_logic_ns % tau_ns;
        if (t1_mid_ns < 0) t1_mid_ns += tau_ns;
        return apply_mu_from_t1mid_or_fit(mu_ns, t1_mid_ns, tau_ns, s_abs_max_ppm, k_capacity);
    }

    // ====== 传统 slew（保留兼容） ======
    inline void slew_towards(int64_t target_off_ns, int64_t slew_rate_ns_per_s) noexcept {
        if (slew_rate_ns_per_s <= 0) { set_offset_ns(target_off_ns); return; }
        int64_t cur  = offset_ns();
        int64_t diff = target_off_ns - cur;
        if (diff == 0) return;
        int64_t step = bounded_slew_step(diff, slew_rate_ns_per_s);
        offset_ns_.fetch_add(step, std::memory_order_relaxed);
        // 高频 slew 不必每步发布；由外层在收敛/周期点调用 publish_anchor_()
    }
    inline void apply_phase_error(int64_t e_ns, double alpha, int64_t slew_ns_per_s) noexcept {
        int64_t cur = offset_ns();
        int64_t target = cur - (int64_t)(alpha * (double)e_ns);
        slew_towards(target, slew_ns_per_s);
    }

    // ====== 频率校正接口（保留占位） ======
    inline void set_ppm(double ppm) noexcept { ppm_.store(ppm, std::memory_order_relaxed); }
    inline double ppm() const noexcept { return ppm_.load(std::memory_order_relaxed); }

private:
    // Slew 步长
    inline int64_t bounded_slew_step(int64_t diff_ns, int64_t slew_rate_ns_per_s) const noexcept {
        using namespace std::chrono;
        const int64_t now_steady_ns =
            duration_cast<nanoseconds>(steady_clock::now().time_since_epoch()).count();

        int64_t prev_ns = last_slew_ns_.load(std::memory_order_relaxed);
        if (prev_ns == 0) {
            last_slew_ns_.store(now_steady_ns, std::memory_order_relaxed);
            int64_t soft = (slew_rate_ns_per_s > 0) ? (slew_rate_ns_per_s / 10) : 0;
            int64_t step = detail_compat::clamp_compat<int64_t>(diff_ns, -soft, +soft);
            return step == 0 ? (diff_ns > 0 ? 1 : -1) : step;
        }

        int64_t dt_ns = now_steady_ns - prev_ns;
        if (dt_ns <= 0) dt_ns = 1;
        last_slew_ns_.store(now_steady_ns, std::memory_order_relaxed);

        long double max_step = (long double)slew_rate_ns_per_s * (long double)dt_ns / 1e9L;
        long double step_ld  = detail_compat::clamp_compat<long double>((long double)diff_ns, -max_step, +max_step);
        if (step_ld == 0.0L) step_ld = (diff_ns > 0 ? 1.0L : -1.0L);
        return (int64_t)step_ld;
    }

    // ====== LSE（一次函数拟合） ======
    struct AffineFit { long double a{1.0L}; long double b{0.0L}; bool ok{false}; };

    inline void init_lsq_window(int k) noexcept {
        lsq_cap_  = std::max(3, k);
        lsq_head_ = 0;
        lsq_buf_.clear();
        lsq_buf_.reserve(lsq_cap_);
    }

    inline bool lsq_fit(AffineFit& out) const noexcept {
        const int n = (int)lsq_buf_.size();
        if (n < 3) return false;

        // 中心化
        long double sx=0.0L, sy=0.0L;
        for (int i=0;i<n;++i) { sx += (long double)lsq_buf_[i].first; sy += (long double)lsq_buf_[i].second; }
        const long double mx = sx / (long double)n;
        const long double my = sy / (long double)n;

        long double Sxx=0.0L, Sxy=0.0L;
        for (int i=0;i<n;++i) {
            const long double x = (long double)lsq_buf_[i].first  - mx;
            const long double y = (long double)lsq_buf_[i].second - my;
            Sxx += x*x;  Sxy += x*y;
        }
        if (Sxx <= 0.0L) return false;

        long double a = Sxy / Sxx;      // slope (ns/ns)
        long double b = my - a * mx;    // intercept

        // 斜率保护：|a-1| ≤ 1000 ppm
        const long double A_MAX = 1000e-6L;
        long double da = a - 1.0L;
        if (da >  A_MAX) a = 1.0L + A_MAX;
        if (da < -A_MAX) a = 1.0L - A_MAX;

        out = {a, b, true};
        return true;
    }

    // 仅求“斜率”作为 s（= a-1）
    inline bool lsq_fit_slope_only(double& s_out) const noexcept {
        AffineFit f;
        if (!lsq_fit(f)) return false;
        s_out = (double)(f.a - 1.0L);
        return true;
    }

    // 过锚点 A(RA,LA) 的最小二乘：约束 b = LA - a*RA，仅拟合 a
    inline bool lsq_fit_slope_through_anchor_(int64_t RA, int64_t LA, long double& a_out) const noexcept {
        const int n = (int)lsq_buf_.size();
        if (n < 3) return false;

        long double num = 0.0L, den = 0.0L; // a = Σ(Ri-RA)(Li-LA) / Σ(Ri-RA)^2
        for (int i=0; i<n; ++i) {
            const long double dx = (long double)lsq_buf_[i].first  - (long double)RA;
            const long double dy = (long double)lsq_buf_[i].second - (long double)LA;
            num += dx * dy;
            den += dx * dx;
        }
        if (den <= 0.0L) return false;

        long double a = num / den;

        // 斜率保护：|a-1| ≤ 1000 ppm
        const long double A_MAX = 1000e-6L;
        long double da = a - 1.0L;
        if (da >  A_MAX) a = 1.0L + A_MAX;
        if (da < -A_MAX) a = 1.0L - A_MAX;

        a_out = a;
        return true;
    }

    // 把 (R,L) 放入 LSE 环形窗口；必要时重置容量
    inline void lsq_add_sample_(int64_t R, int64_t L, int k_capacity) noexcept {
        if (lsq_cap_ != k_capacity) init_lsq_window(std::max(3, k_capacity));
        if ((int)lsq_buf_.size() < lsq_cap_) {
            lsq_buf_.push_back({R, L});
        } else {
            lsq_buf_[lsq_head_] = {R, L};
            lsq_head_ = (lsq_head_ + 1) % lsq_cap_;
        }
    }

    // ====== 本地计算 + 共享锚点发布 ======
    inline int64_t logical_at_raw_(int64_t raw) const noexcept {
        int64_t L = raw + offset_ns_.load(std::memory_order_relaxed);
        const double  s  = slope_ns_per_ns_.load(std::memory_order_relaxed);
        const int64_t a  = slope_anchor_raw_ns_.load(std::memory_order_relaxed);
        if (a != 0 && s != 0.0) {
            const int64_t dt = raw - a;
            L += (int64_t)std::llround((double)dt * s);
        }
        return L;
    }
    inline void publish_anchor_() noexcept {
        const int64_t R = raw_monotonic_ns();
        publish_anchor_at_raw_(R);
    }
    inline void publish_anchor_at_raw_(int64_t R) noexcept {
        const int64_t L = logical_at_raw_(R);
        const double   s = slope_ns_per_ns_.load(std::memory_order_relaxed);
        shm_pub_.publish((uint64_t)R, L, s, /*version*/1);
    }

private:
    // 是否启用“启动归零”
    bool zero_base_{true};
    // 归零零点（ns，记录的是 TscSource::now_ns() 的绝对值）
    int64_t t0_ns_{0};

    // —— 相位 & 频率（原有）——
    std::atomic<int64_t> offset_ns_{0};   // 逻辑偏移（ns）
    std::atomic<double>  ppm_{0.0};       // 可选：频率校正（默认不用）

    // 锚点（若启用 ppm 时可用）
    int64_t anchor_tsc_ns_{0};
    int64_t anchor_logic_ns_{0};

    // 上次 slew 的时间戳（steady_clock ns since epoch）
    mutable std::atomic<int64_t> last_slew_ns_{0};

    // —— 追赶斜率（成员变量）——
    std::atomic<double>  slope_ns_per_ns_{0.0};     // s（ns/ns）
    std::atomic<int64_t> slope_anchor_raw_ns_{0};   // 斜率锚点（raw_monotonic_ns）

    // —— 本轮映射出的 s 与 θ（t1_mid，相位）——
    std::atomic<double>  mapped_s_ns_per_ns_{0.0};  // s_map = μ / (τ - t1_mid)
    std::atomic<int64_t> mapped_theta_ns_{0};       // θ := t1_mid（相位，0..τ）

    // —— 最小二乘窗（坏轮拟合用）——
    int lsq_cap_{0};
    int lsq_head_{0};
    std::vector<std::pair<int64_t,int64_t>> lsq_buf_; // (R_i, L_i)

    // —— POSIX shm 发布器 ——
    ipc::ShmLcPublisher shm_pub_;
};

} // namespace tsync
